#ifndef RELOCATION_SLAM_POSE_ESTIMATE_3D2D_H
#define RELOCATION_SLAM_POSE_ESTIMATE_3D2D_H
#include <iostream>
#include "MapPoints.h"
#include <opencv2/core/core.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <g2o/core/base_vertex.h>
#include <g2o/core/base_unary_edge.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/optimization_algorithm_levenberg.h>
#include <g2o/solvers/csparse/linear_solver_csparse.h>
#include <g2o/types/sba/types_six_dof_expmap.h>
#include <chrono>
#include "Frame.h"

#include <opencv2/core/eigen.hpp>
#include "Eigen/Core"
#include "Eigen/Geometry"

using namespace std;
using namespace Eigen;

void bundleAdjustment (
        const vector< cv::Point3f > points_3d,
        const vector< cv::Point2f > points_2d,
        const cv::Mat& K,
        cv::Mat& R, cv::Mat& t )
{
    // 初始化g2o
    typedef g2o::BlockSolver< g2o::BlockSolverTraits<6,3> > Block;  // pose 维度为 6, landmark 维度为 3
    Block::LinearSolverType* linearSolver = new g2o::LinearSolverCSparse<Block::PoseMatrixType>(); // 线性方程求解器
    Block* solver_ptr = new Block ( linearSolver );     // 矩阵块求解器
    g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg ( solver_ptr );
    g2o::SparseOptimizer optimizer;
    optimizer.setAlgorithm ( solver );

    // vertex
    g2o::VertexSE3Expmap* pose = new g2o::VertexSE3Expmap(); // camera pose
    Eigen::Matrix3d R_mat;
    R_mat <<
          R.at<double> ( 0,0 ), R.at<double> ( 0,1 ), R.at<double> ( 0,2 ),
            R.at<double> ( 1,0 ), R.at<double> ( 1,1 ), R.at<double> ( 1,2 ),
            R.at<double> ( 2,0 ), R.at<double> ( 2,1 ), R.at<double> ( 2,2 );
    pose->setId ( 0 );
    pose->setEstimate ( g2o::SE3Quat (
            R_mat,
            Eigen::Vector3d ( t.at<double> ( 0,0 ), t.at<double> ( 1,0 ), t.at<double> ( 2,0 ) )
    ) );
    optimizer.addVertex ( pose );

    int index = 1;
    for ( const cv::Point3f p:points_3d )   // landmarks
    {
        g2o::VertexSBAPointXYZ* point = new g2o::VertexSBAPointXYZ();
        point->setId ( index++ );
        point->setEstimate ( Eigen::Vector3d ( p.x, p.y, p.z ) );
        point->setMarginalized ( true ); // g2o 中必须设置 marg 参见第十讲内容
        optimizer.addVertex ( point );
    }

    // parameter: camera intrinsics
    g2o::CameraParameters* camera = new g2o::CameraParameters (
            K.at<double> ( 0,0 ), Eigen::Vector2d ( K.at<double> ( 0,2 ), K.at<double> ( 1,2 ) ), 0
    );
    camera->setId ( 0 );
    optimizer.addParameter ( camera );

    // edges
    index = 1;
    for ( const cv::Point2f p:points_2d )
    {
        g2o::EdgeProjectXYZ2UV* edge = new g2o::EdgeProjectXYZ2UV();
        edge->setId ( index );
        edge->setVertex ( 0, dynamic_cast<g2o::VertexSBAPointXYZ*> ( optimizer.vertex ( index ) ) );
        edge->setVertex ( 1, pose );
        edge->setMeasurement ( Eigen::Vector2d ( p.x, p.y ) );
        edge->setParameterId ( 0,0 );
        edge->setInformation ( Eigen::Matrix2d::Identity() );
        optimizer.addEdge ( edge );
        index++;
    }
    optimizer.setVerbose ( true );
    optimizer.initializeOptimization();
    optimizer.optimize ( 100 );
}

void poseEstimate3d2d(
        Relocation_Slam::Frame f1,
        Relocation_Slam::Frame f2,
        vector<cv::DMatch> matches,
        cv::Mat K,
        vector<int> f1p_ids,
        vector<int> f2p_ids,
        cv::Mat& R, cv::Mat& t)
{
    vector<cv::Point3f> pts_3d;
    vector<cv::Point2f> pts_2d;

    for ( int i=0;i< matches.size();i++)
    {
        if(f1.mmMapPointsRes[i].x!=0&&f1.mmMapPointsRes[i].y!=0){
            pts_3d.push_back(f1.mmMapPointsRes[i]);
            pts_2d.push_back ( f2.mvKeyPoints[f2p_ids[i]].pt );
        }
    }
    cv::Mat r;
    cv::solvePnP ( pts_3d, pts_2d, K, cv::Mat(), r, t, false ); // 调用OpenCV 的 PnP 求解，可选择EPNP，DLS等方法
    cv::Rodrigues ( r, R ); // r为旋转向量形式，用Rodrigues公式转换为矩阵
    bundleAdjustment ( pts_3d, pts_2d, K, R, t );
}
void poseEstimate3d2d(
        Relocation_Slam::Frame f1,
        Relocation_Slam::Frame f2,
        vector<pair<int,int>> match_points,
        vector<Relocation_Slam::MapPoint> MapPoints,
        cv::Mat K,
        cv::Mat& R, cv::Mat& t)
{
    vector<cv::Point3f> pts_3d;
    vector<cv::Point2f> pts_2d;
    for ( int i=0;i< match_points.size();i++)
    {
        if(f1.mbKpTriangle[match_points[i].first] == true){
            pts_3d.push_back(cv::Point3f((float)MapPoints[f1.mnMappointId[match_points[i].first]].mPose[0],
                                         (float)MapPoints[f1.mnMappointId[match_points[i].first]].mPose[1],
                                         (float)MapPoints[f1.mnMappointId[match_points[i].first]].mPose[2]));
            pts_2d.push_back(f2.mvKeyPoints[match_points[i].second].pt);
        }
    }

    cv::Mat r;
    cv::solvePnP ( pts_3d, pts_2d, K, cv::Mat(), r, t, false ); // 调用OpenCV 的 PnP 求解，可选择EPNP，DLS等方法
    cv::Rodrigues ( r, R ); // r为旋转向量形式，用Rodrigues公式转换为矩阵
    bundleAdjustment ( pts_3d, pts_2d, K, R, t );
}
#endif //RELOCATION_SLAM_POSE_ESTIMATE_3D2D_H
